clear
format long
boat_x = [0 1];                       % define the end points of the stick figure
boat_y = [0 0];
desiredH_x = [1 20];
desiredH_y = [0 0];
diffCounter = 0;

axis([-79.95 -79.946 40.434 40.438]);
hold on;
boatHDL=plot(boat_x,boat_y,'EraseMode','background'); % connect the end points,
                                               % and use "EraseMode" to REDUCE FLASHING
desiredHHDL = plot(desiredH_x, desiredH_y, '--','EraseMode','background'); 

headHDL = line('XData',boat_x(1), 'YData',boat_y(1), 'Color','b', ...
    'Marker','o', 'MarkerSize',6, 'LineWidth',2);
headTxt = text(boat_x(1), boat_y(1), sprintf('(%.3f,%.3f)',boat_x(1),boat_y(1)), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','right', 'VerticalAlignment','top');

goalHDL = line('XData',0, 'YData',0, 'Color','r', ...
    'Marker','d', 'MarkerSize',6, 'LineWidth',2);
goalTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

obsHDL = line('XData',0, 'YData',0, 'Color','b', ...
    'Marker','p', 'MarkerSize',6, 'LineWidth',2);
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

%fp = fopen('~/autoBoat/log/log-1370885980.txt');
fp = fopen('~/autoBoat/log/log-1370888790.txt');

c = fscanf(fp,'%s');
fclose(fp);
indexLat = strfind(c, '<lat>')+length('<lat>');
indexLatE = strfind(c, '</lat>') -1;
indexLon = strfind(c, '<lon>')+length('<lon>');
indexLonE = strfind(c, '</lon>') -1;
indexCompass = strfind(c, '<compass>')+length('<compass>');
indexCompassE = strfind(c, '</compass>') -1;
indexDesiredHeading = strfind(c, '<desiredHeading>')+length('<desiredHeading>');
indexGoalLat = strfind(c, '<latitude>')+length('<latitude>');
indexGoalLatE = strfind(c, '</latitude>') -1;
indexGoalLon = strfind(c, '<longitude>')+length('<longitude>');
indexGoalLonE = strfind(c, '</longitude>') -1;
indexObsLat = strfind(c, '<obsLat>')+length('<obsLon>');
indexObsLatE = strfind(c, '</obsLat>') -1;
indexObsLon = strfind(c, '<obsLon>')+length('<obsLon>');
indexObsLonE = strfind(c, '</obsLon>') -1;

set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal
xlabel('Longitude');
ylabel('Latitude');

R = 6371000;

for i = 1 : length(indexLon)
    if (2*(i-diffCounter)-1) < length(indexGoalLon)
        if indexLon(i) < indexGoalLon((i-diffCounter))
            diffCounter = diffCounter+1;
            continue;
        end
    else
        break
    end
    lon = str2double(c(indexLon(i):indexLonE(i)));
    lat = str2double(c(indexLat(i):indexLatE(i)));
    compass = str2double(c(indexCompass(i):indexCompassE(i)));
    compass = compass/180*pi;

    desiredHeading = str2double(c(indexDesiredHeading(i-diffCounter):indexDesiredHeading(i-diffCounter)+7));
    
    desiredH = desiredHeading/180*pi;
    boat_y=[lat-0.0001*cos(compass)  lat];
    boat_x=[lon-0.0001*sin(compass)  lon];
    desiredH_y = [lat lat+0.0001*cos(desiredH)];
    desiredH_x = [lon lon+0.0001*sin(desiredH)];
    goalLon = str2double(c(indexGoalLon(i-diffCounter):indexGoalLonE(i-diffCounter)));
    goalLat = str2double(c(indexGoalLat(i-diffCounter):indexGoalLatE(i-diffCounter)));
    obsLon = str2double(c(indexObsLon(i-diffCounter):indexObsLonE(i-diffCounter)));
    obsLat = str2double(c(indexObsLat(i-diffCounter):indexObsLatE(i-diffCounter)));
    
    obsHeading = atan((obsLon - compass*180/pi)/(obsLat - compass*180/pi));
    obsDistance = (obsLat - compass*180/pi) / cos(obsHeading);
    
    obsLat =asin(sin(lat*pi/180)*cos(obsDistance/R) + cos(lat*pi/180)*sin(obsDistance/R)*cos(obsHeading));
    obsLon =lon + atan2(sin(obsHeading)*sin(obsDistance/R)*cos(lat*pi/180), cos(obsDistance/R)-sin(lat*pi/180)*sin(obsLat))*180/pi+ 0.0001;
    obsLat = obsLat *180 /pi - 0.00001;
 
    
    set(boatHDL,'XData',boat_x,'YData',boat_y);       % REDUCE FLASHING
    set(headHDL, 'XData',lon, 'YData',lat);
%    set(headTxt, 'Position',[lon lat], ...
%        'String',sprintf('Boat(%.5f,%.5f)',[lon lat]));
    set(desiredHHDL,'XData',desiredH_x,'YData',desiredH_y);
    
    set(goalHDL, 'XData',goalLon, 'YData',goalLat);
    set(goalTxt, 'Position',[goalLon goalLat], ...
        'String',sprintf('Goal(%.5f,%.5f)',[goalLon goalLat]));
    if ~isnan(obsHeading)
    set(obsHDL, 'XData',obsLon, 'YData',obsLat);
    set(obsTxt, 'Position',[obsLon obsLat], ...
        'String',sprintf('Obstacle(%.5f,%.5f)',[obsLon obsLat]));
    end
    drawnow         % forces Matlab to redraw the plot at the current values
%    M(i) = getframe;
    pause(.3)
end
%movie(M);